//
//  dijkstra.cpp
//  map-matching
//
//  Created by Lv Xin on 16/5/13.
//  Copyright © 2016年 Lv Xin. All rights reserved.
//
//  Personal Homepage: www.davidlvxin.com

#include <stdio.h>
#include "dijkstra.h"
#include "RTreeUtils.h"

using namespace std;

vector<Rectd> rects_node;
vector<Rectd> rects_edge;
RTree<Rect, double, 2, float> tree_node;
RTree<Rect, double, 2, float> tree_edge;
double point_x[nodenum];
double point_y[nodenum];
extern RTree<Rect, double, 2, float>::Iterator it;
extern vector<Rect> searchRTreeResult;

void Init(){
    string filename_node = "node.txt";
    string filename_edge = "edge.txt";
    
    freopen(filename_node.c_str(),"r",stdin);
    int id;
    double x,y;
    while( scanf("%d %lf %lf",&id,&y,&x) != EOF ) {
        Rectd new_node(x,y,x,y,Rect(x,y,id,-1));
        rects_node.push_back(new_node);
        point_x[id] = x;
        point_y[id] = y;
    }
    CreateRTree(rects_node,tree_node);
    fclose(stdin);
    
    freopen(filename_edge.c_str(),"r",stdin);
    int num1, num2;
    double distance;
    while( scanf("%d %d %d %lf",&id,&num1,&num2,&distance) != EOF ) {
        double x1,x2,y1,y2;
        x1 = point_x[num1];
        y1 = point_y[num1];
        x2 = point_x[num2];
        y2 = point_y[num2];
        if ( x1 > x2 ) { double tmp = x1; x1 = x2; x2 = tmp; }
        if ( y1 > y2 ) { double tmp = y1; y1 = y2; y2 = tmp; }
        Rectd new_edge(x1,y1,x2,y2,Rect(num1,num2,id,distance*1000000));
        rects_edge.push_back(new_edge);
    }
    CreateRTree(rects_edge,tree_edge);
    fclose(stdin);
}

void Dijkstra(
              const int numOfVertex,
              const int startVertex,
              double** map,
              double *distance,
              int *prevVertex
              ) {
    vector<bool> isInS;
    isInS.reserve(0);
    isInS.assign(numOfVertex, false);
    for( int i = 0; i < numOfVertex; ++i )
    {
        distance[i] = map[startVertex][i];
        if( map[startVertex][i] < FAR)
            prevVertex[i] = startVertex;
        else
            prevVertex[i] = -1;
    }
    prevVertex[startVertex] = -1;
    isInS[startVertex] = true;
    int u = startVertex;
    for ( int i = 1; i < numOfVertex; i++ )
    {
        int nextVertex = u;
        double tempDistance = FAR;
        for( int j = 0; j < numOfVertex; ++j )
        {
            if((isInS[j] == false) && (distance[j] < tempDistance))
            {
                nextVertex = j;
                tempDistance = distance[j];
            }
        }
        isInS[nextVertex] = true;
        u = nextVertex;
        for (int j =0; j < numOfVertex; j ++)
        {
            if (isInS[j] == false && map[u][j] < FAR)
            {
                double temp = distance[u] + map[u][j];
                if (temp < distance[j])
                {
                    distance[j] = temp;
                    prevVertex[j] = u;
                }
            }
        }
    }
}

vector<int> findpath(int node1, int node2) {
    vector<int> path;
    double x1,x2,y1,y2;
    if ( node1 == node2 ) {
        path.push_back(node1);
        return path;
    }
    x1 = point_x[node1];
    y1 = point_y[node1];
    x2 = point_x[node2];
    y2 = point_y[node2];
    if ( x1 > x2 ) {
        double tmp = x1;
        x1 = x2;
        x2 = tmp;
    }
    if ( y1 > y2 ) {
        double tmp = y1;
        y1 = y2;
        y2 = tmp;
    }
    Rectd s(x1-delta,y1-delta,x2+delta,y2+delta,Rect(0,0,-1,-1));
    SearchRTree(s,tree_edge);
    int node[2*searchRTreeResult.size()];
    int num = 0;
    int j;
    for ( int i = 0; i < searchRTreeResult.size(); i++ ){
        for ( j = 0; j < num; j++ )
            if ( node[j] == searchRTreeResult[i].first ) break;
        if ( j == num ) node[num++] = searchRTreeResult[i].first;
        for ( j = 0; j < num; j++ )
            if ( node[j] == searchRTreeResult[i].second ) break;
        if ( j == num ) node[num++] = searchRTreeResult[i].second;
    }
    double** map = new double*[num];
    for ( int i = 0; i < num; i++ )
        map[i] = new double[num];
    for ( int i = 0; i < num; i++  ){
        for ( int j = 0; j < num; j++ )
            map[i][j] = FAR;
    }
    for ( int i = 0; i < searchRTreeResult.size(); i++ ) {
        if (( searchRTreeResult[i].first == node1 && searchRTreeResult[i].second == node2 ) ||
            ( searchRTreeResult[i].first == node2 && searchRTreeResult[i].second == node1 )) {
            path.push_back(node2);
            return path;
        }
        int x,y;
        for ( int k = 0; k < num; k++ ) {
            if ( node[k] == searchRTreeResult[i].first ) x = k;
            if ( node[k] == searchRTreeResult[i].second ) y = k;
        }
        map[x][y] = searchRTreeResult[i].distance;
    }
    int point1,point2;
    for ( int k = 0; k < num; k++ ) {
        if ( node[k] == node1 ) point1 = k;
        if ( node[k] == node2 ) point2 = k;
    }
    
    double distance[num];
    int preVertex[num];
    Dijkstra(num, point1, map, distance, preVertex);
    int index = point2;
    stack<int> trace;
    while (preVertex[index] != -1) {
        trace.push(preVertex[index]);
        index = preVertex[index];
    }
    if (!trace.empty()) trace.pop();
    else return path;
    path.push_back(node1);
    while (!trace.empty()) {
        path.push_back(node[trace.top()]);
        trace.pop();
    }
    path.push_back(node2);
    for(int i = 0; i < num; i++) delete[] map[i];
    delete[] map;
    return path;
}